/**
  ******************************************************************************
  * @file           motor.c
  * @author         ZohyCao
  * @brief          电机运动控制
  *                 适用于机器人队电控培训,须配合shell命令解释器
  * 使用步骤：
  *         略
  ******************************************************************************
  *
  * COPYRIGHT(c) 2019 ZohyCao
  *
  ******************************************************************************
  */
/* Includes ---------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "stdio.h"
#include "stdarg.h"
#include "string.h"
#include "stdlib.h"
#include "shell.h"
#include "command.h"
#include "motor.h"
#include "stdint.h"

/* Private types ------------------------------------------------------------*/


/* Private variables --------------------------------------------------------*/

motor_msg m1,m2,m3,m4;
int L = 1;
int data_flag=0;
int Processed_data[4]={0};

/* Private function prototypes ----------------------------------------------*/

/* 驱动电机控制初始化 */
void motor_init(void){
    m1.in[0] = 1;
    m2.in[0] = 1;
    m3.in[0] = 1;
    m4.in[0] = 2;
}

/* 驱动电机指令分发 */
void motor(int x,int y,int w){
    m1.in[1] = x+L*w;
    m2.in[1] = -x/2 + 1.732*y/2+L*w;
    m3.in[1] = -x/2 - 1.732*y/2+L*w;
    Can_TxMessage(0,1,m1.ui8,8);
    Can_TxMessage(0,2,m2.ui8,8);
    Can_TxMessage(0,3,m3.ui8,8);
}


/* 舵机位置调整 */

float poi[4]={0.07,0.13,0.18,0.24};
int ppoi=1;
void reload(void){
    TIM3 ->CCR1 = poi[ppoi++]*((TIM3 ->ARR)+1);
    if(ppoi==4){
      ppoi=1;
      TIM3 ->CCR1 = poi[0]*((TIM3 ->ARR)+1);
    }
    printf("ppoi goes to %d\r\n",ppoi);
}

/*  PID调试 */
/*

  float kp=0.7,ki=0,kd=0;
  float P,I,D;
  int err,errSum=0;
  int target = 10;
  int lasterr = 0;

void setSpeed(int T){
      err = T - speed;
      errSum += err*0.01;
      P = kp * err;
      D = kd * (err - lasterr);
      I = ki * errSum;
      TIM4 ->CCR2 = (P+I+D)*((TIM4 ->ARR)+1)-1;
      lasterr = err;
}*/
